#include <buola/cv/io/ckinect.h>
#include <buola/cv/ccloud.h>
#include <buola/app/ccmdline.h>
#include <buola/image/io.h>

using namespace buola;

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);
    
    if(cmd_line().ArgCount()<1)
    {
        msg_info() << "please specify filename\n";
        return 1;
    }
    
    int lNum=1;
    
    if(cmd_line().ArgCount()>=2)
    {
        lNum=to_<int>(cmd_line().Arg(1));
    }

    try
    {
        cvi::CKinect lKinect;
        lKinect.Start();
        start_timer();
        for(int i=0;i<lNum;i++)
        {
            //if(i!=0) sleep(1_s);
            img::CCameraParameters lParams;
            lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
            auto lImages=lKinect.GetOne();
            auto lImage=cvi::make_point_image(lImages.second,lImages.first,lParams);
            save(lImage,io::file(cmd_line().Arg(0)+to_string(i)+".pcd"));
            img::CImage_rgb8 lRGB(lImage.Size());
            for(int i=0;i<lRGB.Height();i++)
                for(int j=0;j<lRGB.Width();j++)
                    lRGB(j,i)=lImage(j,i)->mPixel;
            //save(lRGB,io::file(cmd_line().Arg(0)+to_string(i)+".png"));
            msg_info() << "saved frame " << i << "\n";
            end_timer();
            start_timer();
            
        }
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
